#include <joyos.h>
#include <lcd.h>
#include <buttons.h>
#include <kern/thread.h>

// my headers
#include "umain.h"
#include "define.h"
#include "goalscore.h"
#include "robomove.h"
#include "roboturn.h"
#include "led.h"
#include "bumper.h"
#include "distance.h"
#include "init.h"
#include "skunk.h"
#include "goalscore.h"

// usetup is called during the calibration period. It must return before the
// period ends.
int usetup (void) {
	
	// Point Input
	printf_P (PSTR("\nHello My Boss!"));
	lcd_print_char('1', 0);
	
	printf_P (PSTR("\nBattery Voltage: %d V"), read_battery());
	
	go_click ();
	getGoalScore();
	printf_P (PSTR("\nSet score to %d"), goalPoint);
	go_click ();
	
	// Under IR/LED
	caliUnderLed();
	caliLeftLed();
	caliRightLed();
	
	// Gyro
	printf_P (PSTR("\nPlace robot, press go to calibrate Gyro."));
	go_click ();
	calibrateGyro();
	
	// Goal Servo
	calibrateGoalServo();
 	return 0;
}
 
// Entry point to contestant code.
// Create threads and return 0.
int umain (void) {
	int (*sensorThreadpt)(void) = sensorThread;
	create_thread(sensorThreadpt, 1000, 0, "Sensor");
	actionThread();
	return 0;
}
void actionThread(void){
	printf_P (PSTR("\nClick GO"));
	go_click();
	initPose();
	printf_P (PSTR("\nPlace %d, orient %d"), initPlace, initOrient);
	pause(3000);
	initTurn();
	
	if (initPlace == 0){
		//whiteStrategy();
	}
	else {
		//blackStrategy();
	}
	while(1){
	
	}	
}
int sensorThread(void){
	
	return 0;
}
	

